// -*- C++ -*-
/*!
 * @file  MonteCarloLocalizationLRF.h for PW
 * @brief Monte Carlo Localization using LRF
 * @date  $Date$
 *
 * $Id$
 */

#ifndef MONTECARLOLOCALIZATIONLRF_H
#define MONTECARLOLOCALIZATIONLRF_H

#include <rtm/Manager.h>
#include <rtm/DataFlowComponentBase.h>
#include <rtm/CorbaPort.h>
#include <rtm/DataInPort.h>
#include <rtm/DataOutPort.h>
#include <rtm/idl/BasicDataTypeSkel.h>

#include "intellirobotStub.h"

// GTK
#ifndef _WITHOUT_GUI
 #include <gtk/gtk.h>
 #include "callbacks.h"
#endif


#ifdef CV_DISP
#include "cv.h"
#include "highgui.h"
#endif

// Service implementation headers
// <rtc-template block="service_impl_h">
#include "LocationInfoSVC_impl.h"

// </rtc-template>

// Service Consumer stub headers
// <rtc-template block="consumer_stub_h">
//del #include "MyDataTypeStub.h"
#include "LRFInfoStub.h"
#include "MapInfoStub.h"

// </rtc-template>

#include "structure.h"

using namespace RTC;


class MonteCarloLocalizationLRF
  : public RTC::DataFlowComponentBase
{
 public:
  MonteCarloLocalizationLRF(RTC::Manager* manager);
  ~MonteCarloLocalizationLRF();

  // <rtc-template block="public_attribute">
  
  // </rtc-template>

  // <rtc-template block="public_operation">
  
  // </rtc-template>

  // The initialize action (on CREATED->ALIVE transition)
  // formaer rtc_init_entry() 
  virtual RTC::ReturnCode_t onInitialize();

  // The finalize action (on ALIVE->END transition)
  // formaer rtc_exiting_entry()
  virtual RTC::ReturnCode_t onFinalize();

  // The startup action when ExecutionContext startup
  // former rtc_starting_entry()
  // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id);

  // The shutdown action when ExecutionContext stop
  // former rtc_stopping_entry()
  // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id);

  // The activated action (Active state entry action)
  // former rtc_active_entry()
  virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);

  // The deactivated action (Active state exit action)
  // former rtc_active_exit()
  virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);

  // The execution action that is invoked periodically
  // former rtc_active_do()
  virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);

  // The aborting action when main logic error occurred.
  // former rtc_aborting_entry()
  // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id);

  // The error action in ERROR state
  // former rtc_error_do()
  // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id);

  // The reset action that is invoked resetting
  // This is same but different the former rtc_init_entry()
  // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id);
  
  // The state update action that is invoked after onExecute() action
  // no corresponding operation exists in OpenRTm-aist-0.2.0
  // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id);

  // The action that is invoked when execution context's rate is changed
  // no corresponding operation exists in OpenRTm-aist-0.2.0
  // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id);


 protected:
  // <rtc-template block="protected_attribute">
  
  // </rtc-template>

  // <rtc-template block="protected_operation">
  
  // </rtc-template>

  // Configuration variable declaration
  // <rtc-template block="config_declare">
  int m__num_particles;
  double m__def_pos_x_m;
  double m__def_pos_y_m;
  double m__def_pos_theta_deg;
  double m__std_pos_x_m;
  double m__std_pos_y_m;
  double m__std_pos_theta_deg;
  double m__m_per_pixel;
  short m__screen_offset_x;
  short m__screen_offset_y;
  std::string m__map_format;
  std::string m__map_filename;
  bool m__x_coord_front_lrf_info;
  bool m__x_coord_front_current_pos_init;
  bool m__x_coord_front_current_pos;
  bool m__x_coord_front_odometry;
  double m__scanAngle;//add
  double m__scanStep;//add
  double m__offsetfov;//add

  // </rtc-template>

  // DataInPort declaration
  // <rtc-template block="inport_declare">
  TimedLongSeq m_RangeData;
  InPort<TimedLongSeq> m_RangeDataIn;
  //replace --
  IIS::TimedPose2D m_Odometry;
  InPort<IIS::TimedPose2D> m_OdometryIn;
  IIS::TimedPose2D m_CurrentPosInit;
  InPort<IIS::TimedPose2D> m_CurrentPosInitIn;
  /*  TimedOdometry m_Odometry;
  InPort<TimedOdometry> m_OdometryIn;
  TimedOdometry m_CurrentPosInit;
  InPort<TimedOdometry> m_CurrentPosInitIn;*/
  //-- replace

  // </rtc-template>


  // DataOutPort declaration
  // <rtc-template block="outport_declare">
  // TimedOdometry m_CurrentPos;
  // OutPort<TimedOdometry> m_CurrentPosOut;
  IIS::TimedPose2D m_CurrentPos;
  OutPort<IIS::TimedPose2D> m_CurrentPosOut;

  
  // </rtc-template>

  // CORBA Port declaration
  // <rtc-template block="corbaport_declare">
  RTC::CorbaPort m_LocationInfoPort;
  RTC::CorbaPort m_LRFInfoPort;
  RTC::CorbaPort m_MapInfoPort;
  
  // </rtc-template>

  // Service declaration
  // <rtc-template block="service_declare">
  LocationInfoSVC_impl m_Location;
  
  // </rtc-template>

  // Consumer declaration
  // <rtc-template block="consumer_declare">
  RTC::CorbaConsumer<LRFInfo> m_LRF;
  RTC::CorbaConsumer<MapInfo> m_Map;
  
  // </rtc-template>

 private:
  //GThreadFunc &gui_updater(gpointer data);
  void *gui_updater(void *arg);
  int dummy;
  // <rtc-template block="private_attribute">
  
  // </rtc-template>

  // <rtc-template block="private_operation">
  
  // </rtc-template>
  bool *port_connect_check;
  Mcl *mcl;
  Point pose_mean, pose_std;

  bool findConnectionToMapInfo;
  bool findConnectionToOdometry;

// GTK関連
#ifndef _WITHOUT_GUI
  GuiInterface *gui;
#endif

#ifdef CV_DISP
  IplImage *imOrgmap;
  IplImage *imTrjmap;
  IplImage *imRobmap;
#endif

};


extern "C"
{
  DLL_EXPORT void MonteCarloLocalizationLRFInit(RTC::Manager* manager);
};

#endif // MONTECARLOLOCALIZATIONLRF_H
